﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using NKH.MindSqualls;
using System.Timers;
using NKH.MindSqualls.MotorControl;
using System.Threading;


namespace Steering.Robot
{
    public class MindsquallsControl : RobotControl
    {
        #region fileds

        // motors
        private NxtBrick brick;
        private NxtMotorSync motorPair;
        private NxtMotor fork;

        // light sensor
        private NxtLightSensor nxtLightSensor;
        private bool nxtLightSensorGenerateLight = false;

        // steering scales /??
        private const int TURN_VALUE_SCALE = 10;
        private const int POWER_VALUE_SCALE = 100;
        private const int TACHOLIMIT_VALUE_SCALE = 100;

        // fork control values
        private const uint FORK_TACHO_LIMIT = 440;
        private const sbyte FORK_POWER = 40;

        // Indicates if fork is on top/middle/down position
        private bool isForkUp = true;
        private bool isForkMiddle = false;
        private bool isForkDown = false;

        // Indicates if robot is moving or not
        private bool isRobotStopped = true;

        private const string failMessage = "brick equals to null or not connected";

        #endregion

        #region constructor
        
        /// <summary>
        /// Initialize robot
        /// </summary>
        /// <param name="bluetoothePort">Com port connecting NXT to PC</param>
        public MindsquallsControl(byte bluetoothePort)
        {
            Debug.WriteLine("initializing motors");

            // initialize brick and connecton port
            brick = new NxtBrick(NxtCommLinkType.Bluetooth, bluetoothePort);

            // initialize steering motors
            brick.MotorA = new NxtMotor();
            brick.MotorB = new NxtMotor();
            motorPair = new NxtMotorSync(brick.MotorA, brick.MotorB);

            // initialize fork motor
            fork = new NxtMotor();
            brick.MotorC = fork;

            Debug.WriteLine("initializing motors complete");

            InitializeLightSensor();
        }

        #endregion

        #region override

        /// <summary>
        /// connect robot. This methos also reset position of motor.
        /// </summary>
        /// <returns></returns>
        public override bool Connect()
        {
            Debug.WriteLine("Connecting");
            brick.Connect();
            ResetMotorsPositions();
            Debug.WriteLine("Connecting succesfull");
            return true;
        }

        /// <summary>
        /// Disconets robot
        /// </summary>
        public override void Disconnect()
        {
            Debug.WriteLine("Disconnecting");
            Idle();

            if (brick != null && brick.IsConnected)
            {
                nxtLightSensor.GenerateLight = false;
                ResetForkPosition();
                brick.Disconnect();
                Debug.WriteLine("Disconnect successful");
            }
            else
            {
                Debug.WriteLine("Disconnect failed");
                Debug.WriteLine(failMessage);
            }
        }

        /// <summary>
        /// Idle all robots movement
        /// </summary>
        public override void Idle()
        {
            Debug.WriteLine("Idle");
            IdleFork();
            IdleMove();
        }

        /// <summary>
        /// Idle fork lifting
        /// </summary>
        public override void IdleFork()
        {
            Debug.WriteLine("IdleFork");
            if (brick != null && brick.IsConnected)
            {
                fork.Idle();
                Debug.WriteLine("IdleFork successful");
            }
            else
            {
                Debug.WriteLine("IdleFork failed");
                Debug.WriteLine(failMessage);
            }
        }

        /// <summary>
        /// Idle move of robot
        /// </summary>
        public override void IdleMove()
        {
            Debug.WriteLine("IdleMove");
            if (brick != null && brick.IsConnected)
            {
                motorPair.Idle();
                Debug.WriteLine("IdleMove successful");
            }
            else
            {
                Debug.WriteLine("IdleMove failed");
                Debug.WriteLine(failMessage);
            }
        }

        /// <summary>
        /// This method is responsible for robot movement
        /// </summary>
        /// <param name="power">Power given to the engines</param>
        /// <param name="tachoLimit">Distance of movement. In Degrees </param>
        /// <param name="turnRatio">Turn ratio</param>
        public override void Move(double power, double tachoLimit, double turnRatio)
        {
            if (power == 0)
            {
                isRobotStopped = true;
            }
            else
            {
                isRobotStopped = false;
                fork.PollInterval = 0;
            }

            if (brick != null && brick.IsConnected == true)
            {
                sbyte convertedPower = (sbyte)(-power * POWER_VALUE_SCALE);
                sbyte convertedTurnRatio = (sbyte)(turnRatio * TURN_VALUE_SCALE);
                ushort convertedTachoLimit = (ushort)(tachoLimit * TACHOLIMIT_VALUE_SCALE);

                if (convertedPower < 0) convertedTurnRatio = (sbyte)(-convertedTurnRatio);

                Debug.WriteLine(
                    "power : " + convertedPower.ToString() + "\n" +
                    "tachoLimit : " + convertedTachoLimit.ToString() + "\n" +
                    "turnRatio : " + convertedTurnRatio.ToString());

                motorPair.Run(convertedPower, convertedTachoLimit, convertedTurnRatio);
            }
            else
            {
                Debug.WriteLine("Move failed");
                Debug.WriteLine(failMessage);
            }
        }

        /// <summary>
        /// This method is responsible for position of fork
        /// </summary>
        /// <param name="position">New position of fork</param>
        public override void ControlFork(double position)
        {
            // engine is reversed so some conditions are reversed too
            if (brick != null && brick.IsConnected == true && isRobotStopped == true)
            {
                int choice = (int)position;
                switch (choice)
                {
                    case 1:   
                        if (isForkMiddle == true)
                        {
                            MoveForkFromMiddleToTop();
                        }
                        else if (isForkDown == true)
                        {
                            MoveForkFromDownToMiddle();
                        }
                        break;
                    case 0:
                        if (isForkDown == true)
                        {
                            MoveForkFromDownToMiddle();
                        }
                        else if (isForkUp == true)
                        {
                            MoveForkFromTopToMiddle();
                        }
                        break;
                    case -1:
                        if (isForkMiddle == true)
                        {
                            MoveForkFromMiddleToDown();
                        }
                        else if (isForkUp == true)
                        {
                            MoveForkFromTopToMiddle();
                        }
                        break;
                    default:
                        break;
                }          
            }
            else
            {
                Debug.WriteLine("ControlFork failed");
                Debug.WriteLine(failMessage);
            }
        }

        public override bool NxtLightSensorGenerateLight
        {
            get { return nxtLightSensorGenerateLight; }
            set
            {
                nxtLightSensorGenerateLight = value;
                nxtLightSensor.GenerateLight = nxtLightSensorGenerateLight;
            }
        }

        #endregion

        #region break motors

        /// <summary>
        /// Break wheels and fork
        /// </summary>
        public void BreakeAll()
        {
            BreakSteering();
            BreakFork();
        }

        /// <summary>
        /// Break wheels
        /// </summary>
        public void BreakSteering()
        {
            Debug.WriteLine("BreakSteering");
            if (brick != null && brick.IsConnected)
            {
                motorPair.Brake();
                Debug.WriteLine("BreakSteering successful");
            }
            else
            {
                Debug.WriteLine("BreakSteering failed");
                Debug.WriteLine(failMessage);
            }

        }

        /// <summary>
        /// Break fork
        /// </summary>
        public void BreakFork()
        {
            Debug.WriteLine("BreakFork");
            if (brick != null && brick.IsConnected)
            {
                fork.Brake();
                Debug.WriteLine("BreakFork successful");
            }
            else
            {
                Debug.WriteLine("BreakFork failed");
                Debug.WriteLine(failMessage);
            }

        }

        #endregion

        #region private

        /// <summary>
        /// Initialize Light Sensor. Light sensor must be connected to first port to the NXT brick.
        /// </summary>
        private void InitializeLightSensor()
        {
            Debug.WriteLine("initializing light sensor");
            nxtLightSensor = new NxtLightSensor();
            brick.Sensor1 = nxtLightSensor;
            nxtLightSensor.GenerateLight = nxtLightSensorGenerateLight;
            Debug.WriteLine("initializing light sensor complete");
        }
        
        /// <summary>
        ///  This method move fork from down position to middle position
        /// </summary>
        private void  MoveForkFromDownToMiddle()
        {
            isForkDown = false;
            isForkMiddle = true;
            fork.Run(-FORK_POWER, FORK_TACHO_LIMIT);
            Debug.WriteLine("down to middle");
        }

        /// <summary>
        ///  This method move fork from middle position to top position
        /// </summary>
        private void  MoveForkFromMiddleToTop()
        {
            isForkMiddle = false;
            isForkUp = true;
            fork.Run(-FORK_POWER, FORK_TACHO_LIMIT);
            Debug.WriteLine("middle to up");            
        }

        /// <summary>
        ///  This method move fork from middle position to down position
        /// </summary>
        private void  MoveForkFromMiddleToDown()
        {
            isForkMiddle = false;
            isForkDown = true;
            fork.Run(FORK_POWER, FORK_TACHO_LIMIT);
            Debug.WriteLine("middle to down");
        }

        /// <summary>
        /// This method move fork from top position to middle position
        /// </summary>
        private void MoveForkFromTopToMiddle()
        {
            isForkUp = false;
            isForkMiddle = true;
            fork.Run(FORK_POWER, FORK_TACHO_LIMIT);
            Debug.WriteLine("up to middle");
        }
         
        #region reset position

        /// <summary>
        /// Reset wheels and fork tachometer
        /// </summary>
        private void ResetMotorsPositions()
        {
            ResetSteeringPosition();
            ResetForkPosition();
        }

        /// <summary>
        /// Reset wheels tachometer
        /// </summary>
        private void ResetSteeringPosition()
        {
            Debug.WriteLine("ResetSteeringPosition");
            if (brick != null && brick.IsConnected)
            {
                motorPair.ResetMotorPosition(true);
                Debug.WriteLine("ResetSteeringPosition successful");
            }
            else
            {
                Debug.WriteLine("ResetSteeringPosition failed");
                Debug.WriteLine(failMessage);
            }
        }

        /// <summary>
        /// Reset fork tachometer 
        /// </summary>
        private void ResetForkPosition()
        {
            Debug.WriteLine("ResetForkPosition");
            if (brick != null && brick.IsConnected)
            {
                try
                {
                    fork.ResetMotorPosition(true);
                    Debug.WriteLine("ResetForkPosition successful");
                }
                catch (Exception exc)
                {
                    Debug.WriteLine(exc.Message);
                }
            }
            else
            {
                Debug.WriteLine("ResetForkPosition failed");
                Debug.WriteLine(failMessage);
            }
        }

        #endregion
        #endregion
    }
}
